#ifndef INFRASTRUCTURE_VEHICLE_MKLM_COMMAND_IMPL_H
#define INFRASTRUCTURE_VEHICLE_MKLM_COMMAND_IMPL_H

#include "kernel/agv/i_agv_command.h"
#include "kernel/interfaces/ILogger.h"
#include "infrastructure/vehicle/robokit_api/RobProtocol.h"
#include "infrastructure/vehicle/robokit_api/RobotTask.h"
#include <QObject>
#include <QString>
#include <memory>
#include <unordered_map>

namespace infrastructure {
namespace vehicle {
namespace mklm {

/**
 * @brief MKLM AGV 命令实现类
 * @details 为每个MKLM AGV维护2个主动连接：
 *          - task_server (RProtocol) 连接到 AGV:29204，发送控制命令
 *          - robo_task (RobotTask) 连接到 AGV:19206，RobotTask API
 */
class MklmCommandImpl : public QObject, public kernel::agv::IAgvCommand {
    Q_OBJECT

public:
    explicit MklmCommandImpl(std::shared_ptr<kernel::ILogger> logger, QObject* parent = nullptr);
    ~MklmCommandImpl() override;

    /**
     * @brief 注册AGV连接（当检测到新MKLM AGV时调用）
     * @param agvNumber AGV编号
     * @param ipAddress AGV的IP地址
     * @return bool 是否成功
     */
    bool registerAgv(int agvNumber, const QString& ipAddress);

    /**
     * @brief 注销AGV连接
     * @param agvNumber AGV编号
     */
    void unregisterAgv(int agvNumber);

    // ========== MKLM特有功能（不在通用接口中） ==========

    /**
     * @brief 设置最终操作（MKLM特有）
     * @details 在下次 moveToStationList 时，会在指定站点添加 operation 字段
     * @param agvNumber AGV编号
     * @param stationId 操作站点ID
     * @param operationName 操作名称（如 "JackHeight", "JackLoad" 等）
     */
    void setFinalOperation(int agvNumber, const std::string& stationId, const std::string& operationName);

    /**
     * @brief 清除最终操作
     * @param agvNumber AGV编号
     */
    void clearFinalOperation(int agvNumber);

    /**
     * @brief 设置门配置映射（MKLM特有，全局配置）
     * @details 用于在 moveToStationList 时自动添加 door_id
     * @param doorIds 站点对 (source, target) -> 门ID 的映射
     */
    void setDoorConfigMap(const std::map<std::pair<std::string, std::string>, int>& doorIds);

    // IAgvCommand 接口实现
    kernel::Result<void> moveToStationList(int agvNumber, const std::vector<std::string>& stations, const std::string& operation = "") override;
    kernel::Result<void> moveToStation(int agvNumber, const std::string& sourceId, const std::string& targetId) override;
    kernel::Result<void> cancelTask(int agvNumber) override;
    kernel::Result<void> pauseTask(int agvNumber) override;
    kernel::Result<void> resumeTask(int agvNumber) override;

    kernel::Result<void> controlJack(int agvNumber, bool raise) override;
    kernel::Result<void> startCharging(int agvNumber, const std::string& chargePoint) override;
    kernel::Result<void> stopCharging(int agvNumber) override;
    kernel::Result<void> controlDoor(int agvNumber, int doorId, bool open) override;

    kernel::Result<void> requestCurrentMap(int agvNumber) override;
    kernel::Result<void> switchMap(int agvNumber, const std::string& mapName, const std::string& switchPoint) override;

private:
    /**
     * @brief AGV连接信息
     */
    struct AgvConnection {
        QString ipAddress;
        std::unique_ptr<robokit::RProtocol> taskServer;  // 连接到 AGV:29204
        std::unique_ptr<robokit::RobotTask> roboTask;    // 连接到 AGV:19206
    };

    /**
     * @brief MKLM特有：最终操作信息
     */
    struct FinalOperation {
        std::string stationId;
        std::string operationName;
    };

    std::shared_ptr<kernel::ILogger> m_logger;
    std::unordered_map<int, AgvConnection> m_agvConnections;

    // MKLM特有配置
    std::unordered_map<int, FinalOperation> m_finalOperations;  // AGV编号 -> 最终操作
    std::map<std::pair<std::string, std::string>, int> m_doorConfigMap;  // 站点对 -> 门ID

    /**
     * @brief 获取AGV的连接信息
     * @param agvNumber AGV编号
     * @return AgvConnection* 连接信息指针，未找到返回nullptr
     */
    AgvConnection* getConnection(int agvNumber);

    // 协议命令类型定义（参考三方代码）
    static constexpr uint16_t CMD_MOVE_TASK_LIST = 2003;
    static constexpr uint16_t CMD_MOVE_TASK = 2009;
    static constexpr uint16_t CMD_CANCEL_TASK = 2008;
    static constexpr uint16_t CMD_PAUSE_TASK = 2010;
    static constexpr uint16_t CMD_RESUME_TASK = 2011;
    static constexpr uint16_t CMD_JACK_CONTROL = 2005;
    static constexpr uint16_t CMD_START_CHARGE = 2007;
    static constexpr uint16_t CMD_STOP_CHARGE = 2012;
    static constexpr uint16_t CMD_DOOR_CONTROL = 2013;
    static constexpr uint16_t CMD_GET_MAP = 2004;
};

} // namespace mklm
} // namespace vehicle
} // namespace infrastructure

#endif // INFRASTRUCTURE_VEHICLE_MKLM_COMMAND_IMPL_H
